/**
 * @file servo_task.cpp
 * @author LinTeX9527 (lintex9527@yeah.net)
 * @brief 舵机任务，模拟按下快门
 * @version 0.1
 * @date 2024-11-21
 *
 * @copyright Copyright (c) 2024
 *
 */


#include <Arduino.h>
#include <ESP32Servo.h>
#include "servo_task.h"

#define SERVO_PIN       16

#define SNAP_FOREWARD_TIME_MS          1000 /* 正转时间 1000ms */
#define SNAP_BACKWARD_TIME_MS          1000 /* 反转时间 1000ms */

/* Data Types ------------------------------------------------------------------------------------*/

typedef enum {
	SNAP_ACTION_INIT = 0,
	SNAP_ACTION_FORWARD = 1,
	SNAP_ACTION_BACKWARD = 2,
	SNAP_ACTION_COMPLETE = 3
} snap_action_e;

/* Variables -------------------------------------------------------------------------------------*/

static bool my_snap_flag = false; /* true 表示要拍照； false 表示不拍照 */
static snap_action_e my_snap_action = SNAP_ACTION_INIT;
static unsigned long my_snap_start_time = 0;
static unsigned long my_foreward_time = 0;
static unsigned long my_backward_time = 0;

Servo myServo;

int current_pos = 0; /* variable to store the servo position */
int target_pos = 0;
int step = 1;

void setup_servo(void)
{
	ESP32PWM::allocateTimer(0);
	ESP32PWM::allocateTimer(1);
	ESP32PWM::allocateTimer(2);
	ESP32PWM::allocateTimer(3);
	myServo.setPeriodHertz(50);
	// myServo.attach(SERVO_PIN, 1000, 2000);
	myServo.attach(SERVO_PIN);
}

/**
 * @brief 激活拍照动作
 *
 * @param
 */
void servo_snap_activate(void)
{
	Serial.println("servo_snap_activate() in");
	my_snap_flag = true;
	my_snap_action = SNAP_ACTION_INIT;
	Serial.println("servo_snap_activate() out");

	Serial.println("SNAP_ACTION_INIT | START");
}

/**
 * @brief 控制舵机转动，模拟按压快门的动作。
 * 1. 舵机正转1秒钟；
 * 2. 舵机翻转1秒钟；
 * 3. 结束
 *
 * @param
 */
void loop_servo_task(void)
{
	if (my_snap_flag) {
		switch (my_snap_action)
		{
		case SNAP_ACTION_INIT:
			my_snap_start_time = millis();
			my_snap_action = SNAP_ACTION_FORWARD;
			Serial.println("SNAP_ACTION_INIT --> SNAP_ACTION_FORWARD");
			break;

		case SNAP_ACTION_FORWARD:
			myServo.write(70);
			my_foreward_time = millis() - my_snap_start_time;
			if (my_foreward_time >= SNAP_FOREWARD_TIME_MS) { // 正转时间满足了 --> 反转
				my_snap_action = SNAP_ACTION_BACKWARD;
				Serial.println("SNAP_ACTION_FORWARD --> SNAP_ACTION_BACKWARD");
			}
			break;

		case SNAP_ACTION_BACKWARD:
			myServo.write(110);
			my_backward_time = millis() - my_snap_start_time;
			if (my_backward_time >= (SNAP_FOREWARD_TIME_MS + SNAP_BACKWARD_TIME_MS)) { // 反转时间也满足了
				my_snap_action = SNAP_ACTION_COMPLETE;
				Serial.println("SNAP_ACTION_BACKWARD --> SNAP_ACTION_COMPLETE");
			}
			break;

		case SNAP_ACTION_COMPLETE:
			// 停止舵机
			myServo.write(90);
			my_snap_flag = false;
			my_snap_action = SNAP_ACTION_INIT; // 初始化下一次条件
			Serial.println("SNAP_ACTION_COMPLETE --> SNAP_ACTION_INIT | STOP");
			break;

		default:
			break;
		}
	}
}
